import copy
import math
import sys
from math import factorial
import time

import numpy as np
import plotly.graph_objs as go
import plotly.offline as of

space = 0.1  # 点间距期望
bezier_point_num = 4  # 贝塞尔间隔点数
bezier_point_gap_num = 0  # 贝塞尔间隙点数
space_detal = 0.008  # 点间距容许误差
digui_time = 0
lon0, lat0 = 0, 0

R_e = 6378137.0  # 椭球体长半轴
R_f = 6356752.314245  # 椭球体短半轴
e_1 = math.sqrt(pow(R_e, 2) - pow(R_f, 2)) / R_e  # 第一偏心率

write_path = str(r'.\maping2h.txt')  # 读图文件位置
open_path = str(r".\maping2.txt")  # 写图文件位置


def ll_to_xy(lon, lat):
    sin_lat1 = math.sin(math.radians(lat0))
    cos_lat1 = math.cos(math.radians(lat0))
    square_e = e_1 * e_1
    square_sin_lat1 = sin_lat1 * sin_lat1

    R_n = R_e / (math.sqrt(1 - square_e * square_sin_lat1))  # 卯酉面等效曲率半径(lon1, lat1)
    R_m = (R_n * (1 - square_e)) / (1 - square_e * square_sin_lat1)  # 子午面等效曲率半径(lon1, lat1)

    gx = math.radians(lon - lon0) * R_n * cos_lat1
    gy = math.radians(lat - lat0) * R_m
    return gx, gy


def xy_to_ll(x, y):
    sin_lat1 = math.sin(math.radians(lat0))
    cos_lat1 = math.cos(math.radians(lat0))
    square_e = e_1 * e_1
    square_sin_lat1 = sin_lat1 * sin_lat1

    R_n = R_e / (math.sqrt(1 - square_e * square_sin_lat1))  # 卯酉面等效曲率半径(lon1, lat1)
    R_m = (R_n * (1 - square_e)) / (1 - square_e * square_sin_lat1)  # 子午面等效曲率半径(lon1, lat1)

    lat = y / 3.1415927 * 180 / R_m + lat0
    lon = x / 3.1415927 * 180 / R_n / cos_lat1 + lon0

    return lon, lat


def Trans_angle_to_heading(angle):
    angle = 360 - angle
    angle = angle + 90
    if angle >= 360:
        angle = angle - 360
    return angle


# 读取txt文件
def openreadtxt(file_name):
    global lat0, lon0
    data = []
    file = open(file_name, 'r')  # 打开文件
    file_data = file.readlines()  # 读取所有行
    for row in file_data:
        tmp_list = row.split(' ')  # 按‘，’切分每行的数据
        tmp_list[-1] = tmp_list[-1].replace(',\n', '')  # 去掉换行符
        tmp_list = tmp_list[0].split(',')
        temp_list = []

        for i in tmp_list:
            temp_list.append(float(i))
        loni, lati = temp_list[0], temp_list[1]
        if lon0 == 0 and lat0 == 0:
            lon0, lat0 = temp_list[0], temp_list[1]
        temp_list[0], temp_list[1] = ll_to_xy(loni, lati)
        if len(temp_list) == 17:
            temp_list.append(1)
            temp_list.append(1)
            temp_list.append(3)
            temp_list.append(2)
        data.append(temp_list)  # 将每行数据插入data中
    return data


# 写txt文本
def writetxt(maping):
    path = write_path
    file = open(path, 'w+')
    i = 0
    for mapingi in maping:
        i = i + 1
        if i % 10 == 0:
            mapi = str()
            loni, lati = xy_to_ll(mapingi[0], mapingi[1])
            mapi += str("%.8f" % loni) + "," + "%.8f" % lati + ","  # 纬度和经度
            mapi += str("%.3f" % mapingi[14]) + ","  # 航向
            mapi += str(int(mapingi[15])) + ","  # GPS时间
            mapi += "\n"
            file.write(mapi)


def Cal_Vector_length(vx, vy):
    return (vx ** 2 + vy ** 2) ** 0.5


def Vector_doc_product(v1x, v1y, v2x, v2y):
    return v1x * v2x + v1y * v2y


def Xianduan_to_Vector(p1x, p1y, p2x, p2y):
    return p2x - p1x, p2y - p1y


def Caldis(x1, x2, y1, y2):
    return ((x1 - x2) ** 2 + (y1 - y2) ** 2) ** 0.5


def Get_nearest_index(px, py, maping_spacing):
    dis_near = 100
    nearest_index = -1
    for index, mi in enumerate(maping_spacing):
        dis_temp = Caldis(px, mi[0], py, mi[1])
        if dis_temp < dis_near:
            dis_near = dis_temp
            nearest_index = index
    return nearest_index


def Getpointnum(points, space):
    dis_straight = 0
    x_last, y_last = points[0][0], points[0][1]
    for pointi in points:
        # print("dis1 = {}".format(dis_straight))
        # print("caldis = ", pointi[0], x_last, pointi[1], y_last)
        dis_straight += Caldis(pointi[0], x_last, pointi[1], y_last)
        # print("dis2 = {}".format(dis_straight))

        x_last, y_last = pointi[0], pointi[1]
        # print("x-last, y_last = ", x_last, y_last)
    # print("dis = {}, space = {}".format(dis_straight, space))
    numf = int(20 * dis_straight / space)
    print("起终点折线距离 = {}，初始点数 = {}".format(dis_straight, numf))
    return numf


def Calangle(x1, x2, y1, y2):
    if x1 == x2:
        if y2 >= y1:
            angle = 90
        else:
            angle = -90
    else:
        angle = math.atan((y2 - y1) / (x2 - x1))
        angle = math.degrees(angle)
        if x2 < x1:
            if y2 < y1:
                angle = angle - 180
            else:
                angle = angle + 180

    return angle


def Select_path_point(px, py, space):
    x0, y0 = px[0], py[0]
    x, y, h = [], [], []
    for i in range(len(px)):
        dis = Caldis(px[i], x0, py[i], y0)
        if dis >= space or dis == 0:
            x.append(px[i])
            y.append(py[i])
            x0, y0 = px[i], py[i]
            if i == 0:
                angle = Calangle(px[0], px[1], py[0], py[1])
            elif i == len(px) - 1:
                angle = Calangle(px[i - 1], px[i], py[i - 1], py[i])
            else:
                angle = Calangle(px[i - 1], px[i + 1], py[i - 1], py[i + 1])
            hi = Trans_angle_to_heading(angle)
            h.append(round(hi, 3))
    return x, y, h


def Getpath(points, numf, space):
    N = len(points)  # 控制点个数
    n = N - 1  # 阶数
    px, py = [], []
    # print("numf=", numf)
    for T in range(numf):
        t = T / numf
        x, y = 0, 0

        for i in range(N):
            B = factorial(n) * t ** i * (1 - t) ** (n - i) / (factorial(i) * factorial(n - i))
            x += points[i][0] * B
            y += points[i][1] * B
        px.append(x)
        py.append(y)
    # print("初始坐标点的数量 = {}".format(len(px)))
    px, py, ph = Select_path_point(px, py, space)
    return px, py, ph


def Cal_control_point(pointa, pointb, s):
    # print("pointa = ", pointa)
    # print("pointb = ", pointb)
    # print("s = ", s)
    va = np.array([pointa[0], pointa[1]])
    vb = np.array([pointb[0], pointb[1]])
    # 计算向量ab
    vab = vb - va
    # print("vab = ", vab)
    if math.sqrt(math.pow(vab[0], 2) + math.pow(vab[1], 2)) == 0:
        return 0, 0
    # 计算单位向量
    unit_vab = vab / np.linalg.norm(vab)
    if math.isnan(unit_vab[0]) or math.isnan(unit_vab[1]):
        return 0, 0
    # print("unit_vab = ", unit_vab)
    # 计算向量ac
    vac = unit_vab * s
    # 计算c点坐标
    vc = va + vac
    return vc[0], vc[1]


def Xianjie(line1, line2, maping_spacing):
    sk = 0.3
    point1 = [line1[0][0], line1[0][1]]
    point2 = [line1[-1][0], line1[-1][1]]
    point3 = [line2[0][0], line2[0][1]]
    point4 = [line2[-1][0], line2[-1][1]]
    # print("point1 = ", point1)
    # print("point2 = ", point2)
    # print("point3 = ", point3)
    # print("point4 = ", point4)
    s = math.sqrt(math.pow(point4[0] - point1[0], 2) + math.pow(point4[1] - point1[1], 2))
    # print("s = ", s)
    p2cx, p2cy = Cal_control_point(point1, point2, s * sk)
    p3cx, p3cy = Cal_control_point(point4, point3, s * sk)
    # print("point1 = ", point1)
    # print("point2 = ", point2)
    point2c = [p2cx, p2cy]
    point3c = [p3cx, p3cy]
    points = [point1, point2c, point3c, point4]
    print("s = ", s, "points = ", points)
    # sys.exit(1)
    # 获取初始密集点的数量
    numf = Getpointnum(points, space)
    # 获取路径点的x, y, heading
    px, py, ph = Getpath(points, numf, space)
    line_spacing = []
    for i in range(len(px)):
        line_spacing.append(copy.deepcopy(maping_spacing[0]))
    for i in range(len(px)):
        nearest_index = Get_nearest_index(px[i], py[i], maping_spacing)
        line_spacing[i] = copy.deepcopy(maping_spacing[nearest_index])
        # print(line_spacing[i])
        # if len(line_spacing[i]) == 17:
        #     line_spacing[i].append(1)
        #     line_spacing[i].append(1)
        #     line_spacing[i].append(3)
        #     line_spacing[i].append(2)
        # line_spacing[i][17] = 1
        # line_spacing[i][18] = 1
        # line_spacing[i][19] = 3
        # line_spacing[i][20] = 2
        line_spacing[i][0] = px[i]
        line_spacing[i][1] = py[i]
        line_spacing[i][14] = ph[i]
    return line_spacing


def point3s_to_question(point1, point2, point3):
    question_states = []
    # 将三个点按照13 12 23 生成向量
    v1x, v1y = Xianduan_to_Vector(point1[0], point1[1], point3[0], point3[1])
    v2x, v2y = Xianduan_to_Vector(point1[0], point1[1], point2[0], point2[1])
    v3x, v3y = Xianduan_to_Vector(point2[0], point2[1], point3[0], point3[1])
    # 得到向量v1与v2，v2与v3的点乘结果
    v1_doc_v2 = Vector_doc_product(v1x, v1y, v2x, v2y)
    v2_doc_v3 = Vector_doc_product(v2x, v2y, v3x, v3y)
    # 如果上面两个点乘结果都大于0，证明不存在折返点

    # 求13与12的夹角,角度值[0, 180]
    # print(v1_doc_v2, Cal_Vector_length(v1x, v1y), Cal_Vector_length(v2x, v2y))
    # print(v1_doc_v2 / (Cal_Vector_length(v1x, v1y) * Cal_Vector_length(v2x, v2y)))
    if Cal_Vector_length(v2x, v2y) <= 0.02:
        return 0, 0, 0, 0, [1.2]
    elif Cal_Vector_length(v1x, v1y) <= 0.02 or Cal_Vector_length(v3x, v3y) <= 0.02:
        return 0, 0, 0, 0, [1.3]

    # elif Cal_Vector_length(v1x, v1y) == 0 or Cal_Vector_length(v3x, v3y) == 0:
    #     return 0, 0, 0, 0, [1.3]
    # elif Cal_Vector_length(v2x, v2y) < 0.02:
    #     return 0, 0, 0, 0, [1.6]

    # if Cal_Vector_length(v1x, v1y) == 0:
    #     return 0, 0, 0, 0, [1.3]
    # elif Cal_Vector_length(v2x, v2y) == 0 or Cal_Vector_length(v3x, v3y) == 0:
    #     return 0, 0, 0, 0, [1.2]

    v1_radians_v2 = math.acos(round(v1_doc_v2 / (Cal_Vector_length(v1x, v1y) * Cal_Vector_length(v2x, v2y)), 8))
    # v1_degrees_v2 = math.degrees(v1_radians_v2)
    # print("v2_doc_v3", v2_doc_v3)
    # print("Cal_Vector_length(v2x, v2y)", Cal_Vector_length(v2x, v2y))
    # print("Cal_Vector_length(v3x, v3y)", Cal_Vector_length(v3x, v3y))
    v2_radians_v3 = math.acos(round(v2_doc_v3 / (Cal_Vector_length(v2x, v2y) * Cal_Vector_length(v3x, v3y)), 8))

    v2_degrees_v3 = math.degrees(v2_radians_v3)

    # 如果夹角超过90，证明路径点后退了，记为1
    if v2_degrees_v3 > 90:
        question_states.append(2)
    elif v2_degrees_v3 > 1:  # 角度的抖动
        question_states.append(3)
    # if v2_degrees_v3 > 15:
    #     print("point2 > 15", point2)

    # 求中间点到 前后点连线的最小距离
    dis_point2_to_v1 = Cal_Vector_length(v2x, v2y) * math.sin(v1_radians_v2)

    # 求中间点与下一点的距离
    dis_point2_to_point3 = Caldis(point2[0], point3[0], point2[1], point3[1])
    if dis_point2_to_point3 > space + space_detal or dis_point2_to_point3 < space - space_detal:
        question_states.append(5)
    # if dis_point2_to_point3 > 0.1:
    #     print("point2距离过大", point2)

    # # 求中间点的航向 与 前后点航向平均值的 差值
    # detal_heading_point2h_to_point13havg = point2[2] - (point1[2] + point3[2]) / 2
    # if detal_heading_point2h_to_point13havg > 180:
    #     detal_heading_point2h_to_point13havg -= 360
    # elif detal_heading_point2h_to_point13havg < -180:
    #     detal_heading_point2h_to_point13havg += 360

    # 求中间点的航向 与 前后点连线方向的 差值
    v1_radians_xaxis = math.acos(Vector_doc_product(v1x, v1y, 1, 0) / Cal_Vector_length(v1x, v1y))
    v1_degrees_xaxis = math.degrees(v1_radians_xaxis)
    v1_degrees_xaxis = v1_degrees_xaxis if v1y >= 0 else v1_degrees_xaxis * (-1)
    v1_heading = Trans_angle_to_heading(v1_degrees_xaxis)
    detal_heading_point2h_to_point13h = point2[2] - v1_heading
    if detal_heading_point2h_to_point13h >= 180:
        detal_heading_point2h_to_point13h -= 360
    elif detal_heading_point2h_to_point13h <= -180:
        detal_heading_point2h_to_point13h += 360
    # if detal_heading_point2h_to_point13h > 90:
    # print(point2[2], v1_heading, v1x, v1y)

    return v2_degrees_v3, dis_point2_to_v1, dis_point2_to_point3, detal_heading_point2h_to_point13h, question_states


def Replace(i, maping1):
    global bezier_point_num
    if i < 11:
        line1 = [[mi[0], mi[1]] for mi in maping1[0: i]]
        line2 = [[mi[0], mi[1]] for mi in
                 maping1[i + bezier_point_gap_num + 1: i + bezier_point_gap_num + 1 + bezier_point_num]]
        starti, endi = 0, i + bezier_point_gap_num + 1 + bezier_point_num
    elif i > len(maping1) - 12:
        line1 = [[mi[0], mi[1]] for mi in
                 maping1[i - bezier_point_gap_num - bezier_point_num: i - bezier_point_gap_num]]
        line2 = [[mi[0], mi[1]] for mi in maping1[i + 1:]]
        starti, endi = i - 5 - bezier_point_num, len(maping1) - 1
    else:
        line1 = [[mi[0], mi[1]] for mi in
                 maping1[i - bezier_point_gap_num - bezier_point_num: i - bezier_point_gap_num]]
        line2 = [[mi[0], mi[1]] for mi in
                 maping1[i + bezier_point_gap_num + 1: i + bezier_point_gap_num + 1 + bezier_point_num]]
        starti, endi = i - bezier_point_gap_num - bezier_point_num, i + bezier_point_gap_num + bezier_point_num
    maping_spacing = maping1[starti: endi + 1]
    line_spacing = Xianjie(line1, line2, maping_spacing)
    del maping1[starti: endi + 1]
    for linei_point in reversed(line_spacing):
        maping1.insert(starti, linei_point)
    return maping1


# 从前向后处理地图，递归50次后返回
def Handle(maping1, result=None, start_index=1):
    global digui_time
    digui_time += 1
    print("start_index = ", start_index)
    if result is None:
        result = []
    if digui_time > 100:
        print(digui_time)
        return maping1, result, 0
    for i in range(start_index, len(maping1) - 2):
        point1 = [maping1[i - 1][0], maping1[i - 1][1], maping1[i - 1][14]]
        point2 = [maping1[i][0], maping1[i][1], maping1[i][14]]
        point3 = [maping1[i + 1][0], maping1[i + 1][1], maping1[i + 1][14]]
        result_temp = point3s_to_question(point1, point2, point3)
        if result_temp[-1]:
            if i == 1:
                print(result_temp)
                time.sleep(15)
            if min(result_temp[-1]) == 1.3:
                print("前后两点重合", i, result_temp[-1], maping1[i][17], maping1[i][18], maping1[i][0], maping1[i][1])
                maping1.pop(i + 1)
                if i < 5:
                    return Handle(maping1)
                else:
                    result.append(result_temp)
                    return Handle(maping1, result, i - 3)
            elif min(result_temp[-1]) == 1.2:
                print("与中间点重合", i, result_temp[-1], maping1[i][17], maping1[i][18], maping1[i][0], maping1[i][1])
                maping1.pop(i)
                if i < 5:
                    return Handle(maping1)
                else:
                    return Handle(maping1, result, i - 3)
            elif min(result_temp[-1]) == 2:
                maping1.pop(i + 1)
                print("存在回程路径", i, result_temp[-1], maping1[i][17], maping1[i][18], maping1[i][0], maping1[i][1])
                if i < 5:
                    return Handle(maping1)
                else:
                    return Handle(maping1, result, i - 3)
            elif min(result_temp[-1]) == 3:
                print("后点角度过大", i, result_temp[-1], maping1[i][17], maping1[i][18], maping1[i][0], maping1[i][1])
                # 调用Replace函数，替换不合适的点
                maping1 = Replace(i, maping1)
                if i < 5:
                    return Handle(maping1)
                else:
                    return Handle(maping1, result, i - 3)
            elif min(result_temp[-1]) == 5:
                print("点间距不合适", i, result_temp[-1], maping1[i][17], maping1[i][18], maping1[i][0], maping1[i][1])
                maping1 = Replace(i, maping1)
                if i < 5:
                    return Handle(maping1)
                else:
                    return Handle(maping1, result, i - 3)
        else:
            result.append(result_temp)

    return maping1, result, 1


def ana_maping0(maping1):
    result = []
    for i in range(1, len(maping1) - 2):
        point1 = [maping1[i - 1][0], maping1[i - 1][1], maping1[i - 1][14]]
        point2 = [maping1[i][0], maping1[i][1], maping1[i][14]]
        point3 = [maping1[i + 1][0], maping1[i + 1][1], maping1[i + 1][14]]
        result_temp = point3s_to_question(point1, point2, point3)
        # if result_temp[-1] != []:
        #     print(i, result_temp[-1], maping1[i][17], maping1[i][18], maping1[i][0], maping1[i][1])
        result.append(result_temp)
    return result


# 用来画图
def jktdtu(x, y, x0, y0):
    line1 = go.Scatter(x=x, y=y, mode='markers+lines', name='maping0')
    line2 = go.Scatter(x=x0, y=y0, mode='markers+lines', name='mapingh')
    # line3 = go.Scatter(x=line1x, y=line1y, mode='markers+lines', name='前段路径')
    # line4 = go.Scatter(x=line2x, y=line2y, mode='markers+lines', name='后段路径')
    data = [line1, line2]
    fig = go.Figure(data=data)
    fig.update_layout(
        title='路径对比',
        xaxis_title='X坐标（m）',
        yaxis_title='Y坐标（m）',
        width=2000, height=1000,
        paper_bgcolor='rgba(0,0,0,0)',
        plot_bgcolor='rgba(0,0,0,0)',
        xaxis=dict(
            tickmode='linear',
            # tick0=-5,
            dtick=10,
            autorange=False, range=[-300, 1700],
            type='linear',
            gridcolor='rgba(220,220,220,10)',
            zerolinecolor='rgba(50,50,50,10)'
        ),
        yaxis=dict(
            tickmode='linear',
            # tick0=0,
            dtick=10,
            autorange=False, range=[-800, 200],
            type='linear',
            gridcolor='rgba(220,220,220,10)',
            zerolinecolor='rgba(50,50,50,10)'
        )
    )
    of.plot(fig)
    # print(trace)


def test():
    global digui_time
    digui_flag = 0
    time1 = time.time()
    maping0 = openreadtxt(open_path)
    maping1 = copy.deepcopy(maping0)
    result0 = ana_maping0(maping0)
    result = []
    while digui_flag == 0:
        maping1, result, digui_flag = Handle(maping1)
        digui_time = 0

    time2 = time.time()
    print("用时", time2 - time1)
    print("maping0的长度 = {}, maping1的长度 = {}".format(len(maping0), len(maping1)))

    index = [i for i in range(len(result))]
    v2_degrees_v3 = [i[0] for i in result]
    dis_point2_to_v1 = [i[1] for i in result]
    dis_point2_to_point3 = [i[2] for i in result]
    detal_heading_point2h_to_point13h = [i[3] for i in result]

    index0 = [i for i in range(len(result0))]
    v2_degrees_v30 = [i[0] for i in result0]
    dis_point2_to_v10 = [i[1] for i in result0]
    dis_point2_to_point30 = [i[2] for i in result0]
    detal_heading_point2h_to_point13h0 = [i[3] for i in result0]

    print("处理前平均矢量角：", sum(v2_degrees_v30) / len(result0), "处理后平均矢量角：",
          sum(v2_degrees_v3) / len(result))
    print("处理前平均点线距：", sum(dis_point2_to_v10) / len(result0), "处理后平均点线距：",
          sum(dis_point2_to_v1) / len(result))
    print("处理前平均点间距：", sum(dis_point2_to_point30) / len(result0), "处理后平均点间距：",
          sum(dis_point2_to_point3) / len(result))
    #
    # x0 = [i[0] for i in maping0]
    # y0 = [i[1] for i in maping0]
    # x = [i[0] for i in maping1]
    # y = [i[1] for i in maping1]
    # jktdtu(x, y, x0, y0)

    # 写入txt
    print("maping1", len(maping1), len(maping1[0]))
    writetxt(maping1)


if __name__ == '__main__':
    test()
